/*
 * @Author: Colin Dai
 * @Date: 2022-04-30 16:47:05
 * @LastEditors: Colin Dai
 * @LastEditTime: 2022-05-04 20:48:29
 * @FilePath: \intelligent-trash\src\wheel_ctrl.ino
 * @Description: 底盘驱动文件
 * dailianzheng@163.com
 * Copyright (c) 2022 by Colin.Dai, All Rights Reserved. 
 */
#include "wheel_ctrl.h"
//设置是否进行调速
#define ADJUST_SPEED

//最大行驶速度
#define MAX_SPEED 50

int L1_IN1 = 7;int L1_IN2 = 5;//左前轮
int R1_IN1 = 4;int R1_IN2 = 6;//右前轮
int L2_IN1 = 12;int L2_IN2 = 13;//左后轮
int R2_IN1 = 8;int R2_IN2 = 9;//右后轮

//速度控制针脚
#define PIN_SPEED 11

/**
 * @brief 初始化底盘
 * @param {*}
 * @return {*}
 */
void init_wheel() 
{
  Serial.println("init_wheel...");

  pinMode(L1_IN1, OUTPUT);pinMode(L1_IN2, OUTPUT);
  pinMode(R1_IN1, OUTPUT);pinMode(R1_IN2, OUTPUT);
  pinMode(L2_IN1, OUTPUT);pinMode(L2_IN2, OUTPUT);
  pinMode(R2_IN1, OUTPUT);pinMode(R2_IN2, OUTPUT);

  #ifdef ADJUST_SPEED
  Serial.println("Adjust speed enable...");
  pinMode(PIN_SPEED, OUTPUT);
  analogWrite(PIN_SPEED,MAX_SPEED);//目前使用固定速度
  #endif

  allstop();
}

/**
 * @brief 左前轮前进
 * @param {*}
 * @return {*}
 */
void L1_forward()
{
  digitalWrite(L1_IN1,HIGH);
  digitalWrite(L1_IN2,LOW); 
}

/**
 * @brief 右前轮前进
 * @param {*}
 * @return {*}
 */
void R1_forward()
{
  digitalWrite(R1_IN1,HIGH);
  digitalWrite(R1_IN2,LOW);
}

/**
 * @brief 左后轮前进
 * @param {*}
 * @return {*}
 */
void L2_forward()
{
  digitalWrite(L2_IN1,HIGH);
  digitalWrite(L2_IN2,LOW);
}

/**
 * @brief 右后轮前进
 * @param {*}
 * @return {*}
 */
void R2_forward()
{
  digitalWrite(R2_IN1,HIGH);
  digitalWrite(R2_IN2,LOW);
}

/**
 * @brief 全部停止
 * @param {*}
 * @return {*}
 */
void allstop()
{
  digitalWrite(L1_IN1,LOW);
  digitalWrite(L1_IN2,LOW);  
  digitalWrite(R1_IN1,LOW);
  digitalWrite(R1_IN2,LOW);
  digitalWrite(L2_IN1,LOW);
  digitalWrite(L2_IN2,LOW);
  digitalWrite(R2_IN1,LOW);
  digitalWrite(R2_IN2,LOW);
}

/**
 * @brief 左前轮后退
 * @param {*}
 * @return {*}
 */
void L1_backward()
{
  digitalWrite(L1_IN1,LOW);
  digitalWrite(L1_IN2,HIGH);
}

/**
 * @brief 右前轮后退
 * @param {*}
 * @return {*}
 */
void R1_backward()//右前轮后退
{
  digitalWrite(R1_IN1,LOW);
  digitalWrite(R1_IN2,HIGH);
}

/**
 * @brief 左后轮后退
 * @param {*}
 * @return {*}
 */
void L2_backward()//左后轮后退
{
  digitalWrite(L2_IN1,LOW);
  digitalWrite(L2_IN2,HIGH);
}

/**
 * @brief 右后轮后退
 * @param {*}
 * @return {*}
 */
void R2_backward()//右后轮后退
{
  digitalWrite(R2_IN1,LOW);
  digitalWrite(R2_IN2,HIGH);
}


/**
 * @brief 前进
 * @param {*}
 * @return {*}
 */
void car_forward()
{
  Serial.println("car_forward...");
  /*前进*/
  L1_forward();
  R1_forward();
  L2_forward();
  R2_forward();
}

/**
 * @brief 后退
 * @param {*}
 * @return {*}
 */
void car_backward()
{
  Serial.println("car_backward...");
   /*后退*/
  L1_backward();
  R1_backward();
  L2_backward();
  R2_backward();    
}

/**
 * @brief 右转
 * @param {*}
 * @return {*}
 */
void car_right()
{
  Serial.println("car_right...");
   /*顺时针原地旋转*/
  L1_forward();
  R1_backward();
  L2_forward();
  R2_backward();
}

/** 停止
 * @brief 
 * @param {*}
 * @return {*}
 */
void car_stop()
{
  Serial.println("car_stop...");
  allstop();
}

/** 
 * @brief 左转
 * @param {*}
 * @return {*}
 */
void car_left()
{
  Serial.println("car_left...");

  L1_backward();
  R1_forward();
  L2_backward();
  R2_forward();
}

/**测试车轮
 * @param {*}
 * @return {*}
 */
void test_wheel() 
{
  Serial.println("test_wheel...");
 
  /*前进*/
  L1_forward();
  R1_forward();
  L2_forward();
  R2_forward();
  delay(1500);
  allstop();
  delay(1500);

  /*后退*/
  L1_backward();
  R1_backward();
  L2_backward();
  R2_backward();
  delay(1500);
  allstop();
  delay(1500);
  
  /*顺时针原地旋转*/
  L1_forward();
  R1_backward();
  L2_forward();
  R2_backward();
  delay(1500);
  allstop();
  delay(1500);

  /*逆时针原地旋转*/
  L1_backward();
  R1_forward();
  L2_backward();
  R2_forward();
  delay(1500);
  allstop();
  delay(1500);

  /*左边平移*/
  L1_backward();
  R1_forward();
  L2_forward();
  R2_backward();
  delay(1500);
  allstop();
  delay(1500);

  /*右边平移*/
  L1_forward();
  R1_backward();
  L2_backward();
  R2_forward();
  delay(1500);
  allstop();
  delay(1500);
  
  /*斜向左上方*/
  R1_forward();
  L2_forward();
  delay(1500);
  allstop();
  delay(1500);
  
  /*斜向右上方*/
  L1_forward();
  R2_forward();
  delay(1500);
  allstop();
  delay(1500);
  
  /*斜向左下方*/
  L1_backward();
  R2_backward();
  delay(1500);
  allstop();
  delay(1500);
  
  /*斜向右下方*/
  R1_backward();
  L2_backward();
  delay(1500);
  allstop();
  delay(1500);
}
